﻿Imports System.Drawing

Public Class Form1
    Inherits System.Windows.Forms.Form
    Dim lineindex As Integer = 0
    Public homeR, homeA, homeZ, X0, Y0, Z0, X, Y, Z, C, theta, theta0, radius, radius0 As Double  'homeR, homeA, homeZ are coordinate relative to home.
    '    X,Y,Z are coordinates used in calculations and transformations.
    Dim homeR0, homeZ0, homeA0, setRHome As Integer
    Dim rSpd = 0, aSpd = 0, zSpd = 0, rFR = 0, aFR = 0, zFR = 0, feedRate As Integer = 0
    Dim jogSpeed As Single
    Dim angle = 0, rad = 0, r As Double = 0
    Dim comTxt = "JG0,0,0;BGABC", address = "169.254.61.36", PAR, PAA, PAZ, msg, title, fileName, gCode, textLine As String
    Dim read = False, motion = False, nStep = False, jogDirec, feedDirec, rbCSAState, rbCSBState, rbCSCState As Boolean
    Dim eStop As Boolean = False

    'Must add Galil to project reference in COM
    Dim WithEvents g As New Galil.Galil

    Dim style As MsgBoxStyle
    Dim response As MsgBoxResult
    Dim countPerDegree = 12800.0 * 48.0 / 18.0 / 360.0, countPerR = 12800 / 1.714, countPerZ As Double = 12800 / 91.45
    Dim myGraphics As Graphics
    Dim errorR, errorZ, errorA As Double

    'Event runs when Form load's. Connect to controler.
    Private Sub Form1_Load(sender As Object, e As EventArgs) Handles MyBase.Load
        Try
            Label3.Text = g.libraryVersion()
            g.address = address 'Controler IP Address 169.254.0.1
            Label3.Text += g.connection()
            Label3.Text += "MG TIME " + g.command("MG TIME")
        Catch ex As Runtime.InteropServices.COMException
            If ex.Message.Contains("OPEN ERROR") Then
                Label3.Text = ""
                msg = "Unable to connect to Galil Motion Control. Do you want to change address?" ' Define message.
                style = MsgBoxStyle.DefaultButton2 Or MsgBoxStyle.Exclamation Or MsgBoxStyle.YesNoCancel 'Define style
                title = "G Code"   ' Define title.
                response = MsgBox(msg, style, title)  ' Display message.
                If response = MsgBoxResult.Yes Then
                    address = ""
                    Form1_Load(sender, e)
                ElseIf response = MsgBoxResult.No Then
                    Form1_Load(sender, e)
                Else
                    startBtn.Enabled = False
                    resetBtn.Enabled = False
                    arrowsPanel.Enabled = False
                End If
            End If
            Label3.Text += ex.Message
        End Try
        If response <> MsgBoxResult.Cancel Then

            g.command("ST")
            g.command("SH;MT2,2,2;EO1,1,1,1;ER*=1000;")
            X0 = -Math.Round((g.commandValue("TDA") - g.commandValue("TDC")) / countPerR, 3)
            Z0 = g.commandValue("TDC") / countPerZ

            TextBox1.Text = Math.Round(g.commandValue("TDB") / countPerDegree, 3)
            TextBox2.Text = Math.Round(X0, 3)
            TextBox3.Text = Math.Round(Z0, 3)
            rad = g.commandValue("TDB") / countPerDegree * (Math.PI / 180)
            coordXTextBox.Text = Math.Round(X0 * Math.Cos(rad))
            coordYTextBox.Text = Math.Round(X0 * Math.Sin(rad))
            coordZTextBox.Text = TextBox3.Text
            If RB2D.Checked Then
                Y0 = Z0
                Z0 = 0
            Else
                Y0 = X0 * Math.Sin(rad)
                X0 = X0 * Math.Cos(rad)
            End If
            homeR = coordXTextBox.Text
            homeA = coordYTextBox.Text
            homeZ = coordZTextBox.Text
            motionTimer.Interval = 10

        End If
        myGraphics = Graphics.FromHwnd(ActiveForm().Handle)
        drawGraphic()
    End Sub


    'Stops any motion and turns off motor when form closed
    Private Sub Form1_FormClosing(sender As Object, e As FormClosingEventArgs) Handles MyBase.FormClosing
        StopBtn()
        g.command("MO")
    End Sub

    'Draw  Functions
    Function drawGraphic() 'Draws graphic area
        Dim drawFont As New Font("Microsoft Sans Serif", 8)
        Dim drawBrush As New SolidBrush(Color.Black)
        Dim drawFormat As New StringFormat()
        Dim drawString As [String]

        For b As Integer = 60 To 420 Step 10
            myGraphics.DrawLine(Pens.PaleGreen, New Point(10, b), New Point(480, b))
        Next
        For b As Integer = 445 To 685 Step 10
            myGraphics.DrawLine(Pens.PaleGreen, New Point(10, b), New Point(480, b))
        Next

        For a As Integer = 15 To 475 Step 10
            myGraphics.DrawLine(Pens.PaleGreen, New Point(a, 60), New Point(a, 420))
            myGraphics.DrawLine(Pens.PaleGreen, New Point(a, 440), New Point(a, 690))
        Next

        Dim s As Integer = 80
        For b As Integer = 80 To 400 Step 40
            myGraphics.DrawLine(Pens.LimeGreen, New Point(10, b), New Point(480, b))
            drawString = s
            myGraphics.DrawString(drawString, drawFont, drawBrush, 480, b - 5, drawFormat)
            s -= 20
        Next
        s = 60
        For b As Integer = 445 To 685 Step 40
            myGraphics.DrawLine(Pens.LimeGreen, New Point(10, b), New Point(480, b))
            drawString = s
            myGraphics.DrawString(drawString, drawFont, drawBrush, 480, b - 5, drawFormat)
            s -= 20
        Next
        s = -100
        For a As Integer = 45 To 445 Step 40
            myGraphics.DrawLine(Pens.LimeGreen, New Point(a, 60), New Point(a, 420))
            myGraphics.DrawLine(Pens.LimeGreen, New Point(a, 440), New Point(a, 690))
            drawString = s
            myGraphics.DrawString(drawString, drawFont, drawBrush, a - 5, 420, drawFormat)
            drawString = s + 100
            myGraphics.DrawString(drawString, drawFont, drawBrush, a - 5, 690, drawFormat)
            s += 20
        Next

        myGraphics.DrawLine(New Pen(Color.ForestGreen, 2), New Point(10, 240), New Point(480, 240))
        myGraphics.DrawLine(New Pen(Color.ForestGreen, 2), New Point(245, 60), New Point(245, 420))
        myGraphics.DrawLine(New Pen(Color.ForestGreen, 2), New Point(10, 565), New Point(480, 565))
        myGraphics.DrawLine(New Pen(Color.ForestGreen, 2), New Point(45, 440), New Point(45, 690))
        myGraphics.DrawRectangle(New Pen(Color.Black, 3), New Rectangle(New Point(10, 60), New Point(470, 360)))
        myGraphics.DrawRectangle(New Pen(Color.Black, 3), New Rectangle(New Point(10, 440), New Point(470, 250)))
    End Function
    Function drawToPosition() 'Draws the current position of the tool.
        If gCode <> "00" Or "0" Then
            If RB2D.Checked Then
                myGraphics.DrawLine(New Pen(Color.Red, 1), New Point(homeR0 + 245, -homeZ0 + 240), New Point(Math.Round(homeR * 2) + 245, Math.Round(-homeZ * 2) + 240))
                homeR0 = Math.Round(homeR * 2)
                homeZ0 = Math.Round(homeZ * 2)
            Else
                'Draws on the XY-Plane
                myGraphics.DrawLine(New Pen(Color.Red, 1), New Point(homeR0 * Math.Cos(homeA0 * Math.PI / 180) + 245, -homeR0 * Math.Sin(homeA0 * Math.PI / 180) + 240), New Point(homeR * Math.Cos(homeA * Math.PI / 180) + 245, -homeR * Math.Sin(homeA * Math.PI / 180) + 240))
                'Draws on the YZ-Plane
                myGraphics.DrawLine(New Pen(Color.Red, 1), New Point(homeZ0 + 45, -homeR0 * Math.Sin(homeA0 * Math.PI / 180) + 565), New Point(homeZ + 45, -homeR * Math.Sin(homeA * Math.PI / 180) + 565))
                homeA0 = homeA
                homeR0 = Math.Round(homeR)
                homeZ0 = Math.Round(homeZ)
            End If
        End If
    End Function

    '*************************************************Code used when modifying feedrate and jogspeed********************************************************************************
    'Allows user to use feed rate or jog speed as motion speed in manual motion.
    Private Sub RadioButtonFR_CheckedChanged(sender As Object, e As EventArgs) Handles RadioButtonFR.CheckedChanged
        If read = False And nStep = False Then
            If RadioButtonFR.Checked Then
                If motion Then
                    g.command("JG" & rFR & "," & aFR & "," & zFR)
                Else
                    comTxt = ("JG" & rFR & "," & aFR & "," & zFR & ";BGABC")
                End If
            Else
                If motion Then
                    g.command("JG" & rSpd & "," & aSpd & "," & zSpd)
                Else
                    comTxt = ("JG" & rSpd & "," & aSpd & "," & zSpd & ";BGABC")
                End If
            End If
        End If

    End Sub

    'Function checks which jogSpeed have to be changed, and sends new jogSpeed to controler.
    Private Function SetUpJogSpeed()
        If CheckBoxR.Checked Then
            rSpd = Int(jogSpeed * countPerR * 7.5)
        End If
        If CheckBoxA.Checked Then
            aSpd = Int(jogSpeed * countPerDegree * 100)
        End If
        If CheckBoxZ.Checked Then
            zSpd = Int(jogSpeed * countPerZ * 100)
        End If
        If read = False Then
            If motion Then
                g.command("JG" & rSpd & "," & aSpd & "," & zSpd)
            Else
                comTxt = ("JG" & rSpd & "," & aSpd & "," & zSpd & ";BGABC")
            End If
        End If
    End Function

    'Sub called when jogSpeed is manually entered using keyboard. 
    Private Sub jogSpeedTextBox_TextChanged(sender As Object, e As EventArgs) Handles jogSpeedTextBox.TextChanged
        If jogSpeedTextBox.Text = "" Then
            jogSpeed = 0
        Else
            jogSpeed = jogSpeedTextBox.Text / 100
        End If
        SetUpJogSpeed()
    End Sub

    'Subs define actions of plus and minus button if Click, Mousedown, Mouseup. Control jogSpeed of motors
    Private Sub plusBtn_Click(sender As Object, e As EventArgs) Handles plusBtn.Click
        jogSpeed += 0.01
        SetUpJogSpeed()
        jogSpeedTextBox.Text = jogSpeed * 100
    End Sub
    Private Sub plusBtn_Mousedown(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles plusBtn.MouseDown
        Timer2.Interval = 500
        Timer2.Start()
        jogDirec = True
    End Sub
    Private Sub plusBtn_Mouseup(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles plusBtn.MouseUp
        Timer1.Stop()
        Timer2.Stop()
    End Sub
    Private Sub minusBtn_Click(sender As Object, e As EventArgs) Handles minusBtn.Click
        jogSpeed -= 0.01
        SetUpJogSpeed()
        jogSpeedTextBox.Text = jogSpeed * 100
    End Sub
    Private Sub minusBtn_Mousedown(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles minusBtn.MouseDown
        Timer2.Interval = 500
        Timer2.Start()
        jogDirec = False
    End Sub
    Private Sub minusBtn_Mouseup(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles minusBtn.MouseUp
        Timer1.Stop()
        Timer2.Stop()
    End Sub
    Private Sub Timer1_Tick(sender As Object, e As EventArgs) Handles Timer1.Tick

        If jogDirec Then
            jogSpeed += 0.1
        Else
            jogSpeed -= 0.1
        End If
        jogSpeedTextBox.Text = jogSpeed * 100
    End Sub
    Private Sub Timer2_Tick(sender As Object, e As EventArgs) Handles Timer2.Tick
        Timer1.Interval = 100
        Timer1.Start()
    End Sub

    'Function checks which Feed Rate have to be changed, and sends new jogSpeed to controler.
    Private Function SetUpFeedRate()
        If CheckBoxR.Checked Then
            rFR = feedRate
        End If
        If CheckBoxA.Checked Then
            aFR = feedRate
        End If
        If CheckBoxZ.Checked Then
            zFR = feedRate
        End If
        If read = False Then
            If motion Then
                g.command("JG" & rFR & "," & aFR & "," & zFR)
            Else
                comTxt = ("JG" & rFR & "," & aFR & "," & zFR & ";BGABC")
            End If
        End If
    End Function

    'Sub called when Feed Rate is manually entered using keyboard. 
    Private Sub feedRateTextBox_TextChanged(sender As Object, e As EventArgs) Handles feedRateTextBox.TextChanged
        If feedRateTextBox.Text = "" Then
            feedRate = 0
        Else
            feedRate = feedRateTextBox.Text
        End If
        SetUpFeedRate()
    End Sub

    'Subs define actions of plus and minus button if Click, Mousedown, Mouseup. Control jogSpeed of motors
    Private Sub plusFRBtn_Click(sender As Object, e As EventArgs) Handles plusFRBtn.Click
        feedRate += 10
        SetUpFeedRate()
        feedRateTextBox.Text = feedRate
    End Sub
    Private Sub plusFRBtn_Mousedown(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles plusFRBtn.MouseDown
        Timer4.Interval = 500
        Timer4.Start()
        feedDirec = True
    End Sub
    Private Sub plusFRBtn_Mouseup(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles plusFRBtn.MouseUp
        Timer3.Stop()
        Timer4.Stop()
    End Sub
    Private Sub minusFRBtn_Click(sender As Object, e As EventArgs) Handles minusFRBtn.Click
        feedRate -= 10
        SetUpFeedRate()
        feedRateTextBox.Text = feedRate
    End Sub
    Private Sub minusFRBtn_Mousedown(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles minusFRBtn.MouseDown
        Timer4.Interval = 400
        Timer4.Start()
        feedDirec = False
    End Sub
    Private Sub minusFRBtn_Mouseup(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles minusFRBtn.MouseUp
        Timer3.Stop()
        Timer4.Stop()
    End Sub
    Private Sub Timer3_Tick(sender As Object, e As EventArgs) Handles Timer3.Tick
        If feedDirec Then
            feedRate += 50
        Else
            feedRate -= 50
        End If
        feedRateTextBox.Text = feedRate

    End Sub
    Private Sub Timer4_Tick(sender As Object, e As EventArgs) Handles Timer4.Tick
        Timer3.Interval = 100
        Timer3.Start()
    End Sub

    'Preset Feed Rate (fast,normal,slow)
    Private Sub fastFRBtn_Click(sender As Object, e As EventArgs) Handles fastFRBtn.Click
        feedRateTextBox.Text = "600"
    End Sub
    Private Sub normalFRJogBtn_Click(sender As Object, e As EventArgs) Handles normalFRBtn.Click
        feedRateTextBox.Text = "300"
    End Sub
    Private Sub slowFRBtn_Click(sender As Object, e As EventArgs) Handles slowFRBtn.Click
        feedRateTextBox.Text = "100"
    End Sub

    'Preset jogSpeeds (fast,normal,slow)
    Private Sub fastJogBtn_Click(sender As Object, e As EventArgs) Handles fastJogBtn.Click
        jogSpeedTextBox.Text = "85"
    End Sub
    Private Sub normalJogBtn_Click(sender As Object, e As EventArgs) Handles normalJogBtn.Click
        jogSpeedTextBox.Text = "50"
    End Sub
    Private Sub slowJogBtn_Click(sender As Object, e As EventArgs) Handles slowJogBtn.Click
        jogSpeedTextBox.Text = "15"
    End Sub

    'Subs control display of the jogSpeed when RadioButton(A,R,Z) activated
    Private Sub CheckBoxA_CheckedChanged(sender As Object, e As EventArgs) Handles CheckBoxA.CheckedChanged
        If CheckBoxA.Checked Then
            If RadioButtonJS.Checked Then
                jogSpeedTextBox.Text = aSpd / 100 / countPerDegree * 100
            Else
                feedRateTextBox.Text = aFR
            End If
        End If
    End Sub
    Private Sub CheckBoxR_CheckedChanged(sender As Object, e As EventArgs) Handles CheckBoxR.CheckedChanged
        If CheckBoxR.Checked Then
            If RadioButtonJS.Checked Then
                jogSpeedTextBox.Text = rSpd / 7.5 / countPerR * 100
            Else
                feedRateTextBox.Text = rFR
            End If
        End If
    End Sub
    Private Sub CheckBoxZ_CheckedChanged(sender As Object, e As EventArgs) Handles CheckBoxZ.CheckedChanged
        If CheckBoxZ.Checked Then
            If RadioButtonJS.Checked Then
                jogSpeedTextBox.Text = zSpd / 100 / countPerZ * 100
            Else
                feedRateTextBox.Text = zFR
            End If
        End If
    End Sub
    '*************************************************End of code used when modifying feedrate and jogspeed********************************************************************************



    '*****************************************Start, stop and reset button.*********************************************************************************************************
    'When startBtn clicked, comTxt is downloades and motion starts or motion stops.
    Private Sub startBtn_Click(sender As Object, e As EventArgs) Handles startBtn.Click
        If motion = False And comTxt <> "" Then
            startBtn.Text = "STOP"
            startBtn.BackColor = Color.Red
            motionTimer.Start()
            motion = True
            g.programDownload(comTxt)
            g.command("XQ")
        Else
            StopBtn()
        End If
    End Sub
    Private Sub startBtn_MouseDown(sender As Object, e As MouseEventArgs) Handles startBtn.MouseDown
        If startBtn.Text = "STOP" Then
            motion = True
        End If

    End Sub

    'Function called to stop motion.
    Private Function StopBtn()
        startBtn.Text = "START"
        startBtn.BackColor = Color.LimeGreen
        motionTimer.Stop()
        g.command("ST")
        motion = False
        read = False
        'comTxt = ""
        RadioButtonGP.Checked = False
        RadioButtonG0.Checked = False
        RadioButton1.Checked = False
        RadioButtonGC.Checked = False
        Dim TDA = g.commandValue("TDA"), TDB = g.commandValue("TDB"), TDC As Integer = g.commandValue("TDC")
        homeR = -Math.Round((TDA - TDC) / countPerR, 3)
        homeA = Math.Round(TDB / countPerDegree, 3)
        If RB3D.Checked Then
            homeZ = -Math.Round(TDC / countPerZ, 3)
        Else
            homeZ = Math.Round(TDC / countPerZ, 3)
        End If
        drawToPosition()
        TextBox1.Text = homeA
        TextBox2.Text = homeR
        TextBox3.Text = homeZ
        arrowsPanel.Enabled = True
    End Function

    'When resetBtn clicked all changing variable return to 0
    Private Sub resetBtn_Click(sender As Object, e As EventArgs) Handles resetBtn.Click
        StopBtn()
        read = False
        motion = False
        nStep = False
        comTxt = ""
        gCode = ""
        TextBox1.Text = 0
        TextBox2.Text = 0
        TextBox3.Text = 0
        coordXTextBox.Text = 0
        coordYTextBox.Text = 0
        coordZTextBox.Text = 0
        motionTimer.Stop()
        lineindex = 0
        X0 = 0
        Y0 = 0
        Z0 = 0
        X = 0
        Y = 0
        Z = 0
        radius = 0
        radius0 = 0
        theta = 0
        theta0 = 0
        homeA0 = 0
        homeR0 = 0
        homeZ0 = 0
        homeA = 0
        homeR = 0
        homeZ = 0
        errorR = 0
        errorZ = 0
        errorA = 0
        While g.commandValue("TVA") <> "0" Or g.commandValue("TVB") <> "0" Or g.commandValue("TVC") <> "0"
        End While
        g.command("DP0,0,0")
        gCodeTextBox.SelectAll()
        gCodeTextBox.SelectionBackColor = Color.White
        countPerR = 12800 / 1.714
        countPerZ = 12800 / 91.45
        myGraphics.Clear(SystemColors.Control)
        drawGraphic()
        g.command("CSS")
    End Sub

    'Define current position. Used to define the start position of the tool.
    Private Sub zeroABtn_Click(sender As Object, e As EventArgs) Handles zeroABtn.Click
        g.command("DP," & TextBox1.Text * countPerDegree)
        homeA = TextBox1.Text
    End Sub
    Private Sub zeroRBtn_Click(sender As Object, e As EventArgs) Handles zeroRBtn.Click
        g.command("DP" & -TextBox2.Text * countPerR)
        homeR = TextBox2.Text
    End Sub
    Private Sub zeroZBtn_Click(sender As Object, e As EventArgs) Handles zeroZBtn.Click
        g.command("DP,," & -TextBox3.Text * countPerZ)
        homeZ = TextBox3.Text
    End Sub
    Private Sub TextBox1_KeyPress(sender As Object, e As KeyPressEventArgs) Handles TextBox1.KeyPress
        If e.KeyChar = Chr(Keys.Enter) Then
            zeroABtn.PerformClick()
            e.Handled = True
        End If
    End Sub
    Private Sub TextBox2_KeyPress(sender As Object, e As KeyPressEventArgs) Handles TextBox2.KeyPress
        If e.KeyChar = Chr(Keys.Enter) Then
            zeroRBtn.PerformClick()
            e.Handled = True
        End If
    End Sub
    Private Sub TextBox3_KeyPress(sender As Object, e As KeyPressEventArgs) Handles TextBox3.KeyPress
        If e.KeyChar = Chr(Keys.Enter) Then
            zeroZBtn.PerformClick()
            e.Handled = True
        End If
    End Sub

    Private Sub en_Button(status As Boolean)
        startBtn.Enabled = status
        csArbtn.Enabled = status
        jogSpeedTextBox.Enabled = status
    End Sub

    Private Sub E_Stop_Click(sender As Object, e As EventArgs) Handles E_Stop.Click
        If eStop = False Then
            eStop = True
            E_Stop.BackColor = Color.Red
            en_Button(False)
            jogSpeed = 0
            jogSpeedTextBox.Text = jogSpeed
            StopBtn()
        Else
            eStop = False
            E_Stop.BackColor = Color.Transparent
            en_Button(True)
        End If
    End Sub

   
    '*****************************************end of Start, stop and reset button.*********************************************************************************************************


    'Subs control ON/OFF RadioButton(A,B,C)
    Private Sub csArbtn_MouseDown(sender As Object, e As MouseEventArgs) Handles csArbtn.MouseDown
        rbCSAState = csArbtn.Checked
    End Sub

    Private Sub csArbtn_MouseUp(sender As Object, e As MouseEventArgs) Handles csArbtn.MouseUp
        csArbtn.Checked = Not rbCSAState
    End Sub
    Private Sub csBrbtn_MouseDown(sender As Object, e As MouseEventArgs) Handles csBrbtn.MouseDown
        rbCSBState = csBrbtn.Checked
    End Sub
    Private Sub csBrbtn_MouseUp(sender As Object, e As MouseEventArgs) Handles csBrbtn.MouseUp
        csBrbtn.Checked = Not rbCSBState
    End Sub
    Private Sub csCrbtn_MouseDown(sender As Object, e As MouseEventArgs) Handles csCrbtn.MouseDown
        rbCSCState = csCrbtn.Checked
    End Sub
    Private Sub csCrbtn_MouseUp(sender As Object, e As MouseEventArgs) Handles csCrbtn.MouseUp
        csCrbtn.Checked = Not rbCSCState
    End Sub

    '************************************************Code run when cnc is in motion. Used to define when next line must be read.***************************************************
    'Timer runs with motion; refreshing motor positions, detecting if motion completed, and detecting if E-Stop pressed.
    Private Sub motionTimer_Tick(sender As Object, e As EventArgs) Handles motionTimer.Tick
        Dim TDA = g.commandValue("TDA"), TDB = g.commandValue("TDB"), TDC As Integer = g.commandValue("TDC")
        Dim TSA = g.commandValue("TSA"), TSB = g.commandValue("TSB"), TSC As Integer = g.commandValue("TSC")

        homeR = -Math.Round((TDA - TDC) / countPerR, 3)
        homeA = Math.Round(TDB / countPerDegree, 3)
        If RB3D.Checked Then
            homeZ = -Math.Round(TDC / countPerZ, 3)
        Else
            homeZ = Math.Round(TDC / countPerZ, 3)
        End If
        drawToPosition()
        TextBox1.Text = homeA
        TextBox2.Text = homeR
        TextBox3.Text = homeZ
        Dim homex, homey As Double
        rad = homeA * (Math.PI / 180)
        homex = Math.Round(homeR * Math.Cos(rad), 3)
        homey = Math.Round(homeR * Math.Sin(rad), 3)
        If csArbtn.Checked Then
            coordXTextBox.Text = homex - Form2.xA
            coordYTextBox.Text = homey - Form2.yA
            coordZTextBox.Text = homeZ - Form2.zA
        ElseIf csBrbtn.Checked Then
            coordXTextBox.Text = homex - Form2.xB
            coordYTextBox.Text = homey - Form2.yB
            coordZTextBox.Text = homeZ - Form2.zB
        ElseIf csCrbtn.Checked Then
            coordXTextBox.Text = homex - Form2.xC
            coordYTextBox.Text = homey - Form2.yC
            coordZTextBox.Text = homeZ - Form2.zC
        Else
            coordXTextBox.Text = homex
            coordYTextBox.Text = homey
            coordZTextBox.Text = homeZ
        End If
        'X0 = coordXTextBox.Text
        'Y0 = coordYTextBox.Text
        'Z0 = coordZTextBox.Text

        'If TSA = 3 And TSB = 3 And TSC = 3 Then
        ' StopBtn()
        'msg = "Emergency stop activated. Reset to continue."   ' Define message.
        'style = MsgBoxStyle.DefaultButton1 Or MsgBoxStyle.Critical Or MsgBoxStyle.OkOnly 'Define style
        'title = "Emergency Stop"   ' Define title.
        'response = MsgBox(msg, style, title)  ' Display message.
        'End If
        If TSC = 5 Or TSA = 143 Then
            StopBtn()
            msg = "Limit switch activated. Reset to continue."   ' Define message.
            style = MsgBoxStyle.DefaultButton1 Or MsgBoxStyle.Critical Or MsgBoxStyle.OkOnly 'Define style
            title = "Unexpected Stop"   ' Define title.
            response = MsgBox(msg, style, title)  ' Display message.
        End If
        If read Then
            arrowsPanel.Enabled = False
            If motion = False Then
                readNextLine(g.commandValue("LM?"))
            ElseIf gCode = "01" Then
                If g.commandValue("LM?") > 10 Then
                    readNextLine(g.commandValue("LM?"))
                End If
            End If
        ElseIf nStep = False And TDA = PAR And TDB = PAA And TDC = PAZ Then
            StopBtn()
        Else
            arrowsPanel.Enabled = True
        End If

    End Sub

    'Event runs when message received
    Private Sub g_onMessage(ByVal message As String) Handles g.onMessage
        messagesTextBox.AppendText(message) 'print to textbox
        If message.Contains("Motion Completed") And motion = True Then
            motion = False
            nStep = False
            If read = False Then
                StopBtn()
            End If
        End If
        'codeTextBox.Text = motion & message & read
    End Sub
    '************************************************End of Code run when cnc is in motion. Used to define when next line must be read.***************************************************

    '************************************************Manual motion, position and GMC programming**********************************************************************************
    'Define's manually entered motion destination.
    Private Sub RadioButtonGP_CheckedChanged(sender As Object, e As EventArgs) Handles RadioButtonGP.CheckedChanged
        If RadioButtonGP.Checked Then
            PAA = Int(TextBox1.Text * countPerDegree)
            messagesTextBox.Text = PAA
            PAZ = Int(-TextBox3.Text * countPerZ)
            PAR = Int(-TextBox2.Text * countPerR) + -g.commandValue("TDC") - PAZ

            If RadioButtonJS.Checked Then
                comTxt = "PA" & PAR & "," & PAA & "," & PAZ & ";SP" & rSpd & "," & aSpd & "," & zSpd & ";BGABC"
            Else
                comTxt = "PA" & PAR & "," & PAA & "," & PAZ & ";SP" & rFR & "," & aFR & "," & zFR & ";BGABC"
            End If

        End If
    End Sub

    'Send's all motors to zero.
    Private Sub radioButtonG0_CheckedChanged(sender As Object, e As EventArgs) Handles RadioButtonG0.CheckedChanged
        If RadioButtonG0.Checked Then
            PAA = 0
            PAR = 0
            PAZ = 0
            comTxt = ("PA0,0,0;SP" & rSpd & "," & aSpd & "," & zSpd & ";BGABC")
        End If
    End Sub

    'Subs send motion activated by arrows on the interface
    Private Sub plusRBtn_MouseDown(sender As Object, e As MouseEventArgs) Handles plusRBtn.MouseDown
        If RadioButtonJS.Checked Then
            g.command("JG" & Int(jogSpeed * countPerR * 7.5) & ";AC300000;DC700000;BGA")
        Else
            g.command("JG" & VS(1, 0) & ";AC300000;DC700000;BGA")
        End If
        motionTimer.Start()
    End Sub
    Private Sub plusRBtn_MouseUp(sender As Object, e As MouseEventArgs) Handles plusRBtn.MouseUp
        StopBtn()
    End Sub
    Private Sub minusRBtn_MouseDown(sender As Object, e As MouseEventArgs) Handles minusRBtn.MouseDown
        If RadioButtonJS.Checked Then
            g.command("JG-" & Int(jogSpeed * countPerR * 7.5) & ";AC300000;DC700000;BGA")
        Else
            g.command("JG-" & VS(1, 0) & ";AC300000;DC700000;BGA")
        End If

        motionTimer.Start()
    End Sub
    Private Sub minusRBtn_MouseUp(sender As Object, e As MouseEventArgs) Handles minusRBtn.MouseUp
        StopBtn()
    End Sub
    Private Sub clockwiseABtn_MouseDown(sender As Object, e As MouseEventArgs) Handles clockwiseABtn.MouseDown
        If RadioButtonJS.Checked Then
            g.command("JG," & Int(jogSpeed * countPerDegree * 100) & ";AC,300000;DC,700000;BGB")
        Else
            g.command("JG," & feedRate & ";AC,300000;DC,700000;BGB")
        End If

        motionTimer.Start()
    End Sub
    Private Sub clockwiseABtn_MouseUp(sender As Object, e As MouseEventArgs) Handles clockwiseABtn.MouseUp
        StopBtn()
    End Sub
    Private Sub cClockwiseABtn_MouseDown(sender As Object, e As MouseEventArgs) Handles cClockwiseABtn.MouseDown
        If RadioButtonJS.Checked Then
            g.command("JG,-" & Int(jogSpeed * countPerDegree * 100) & ";AC,300000;DC,700000;BGB")
        Else
            g.command("JG,-" & Int(feedRate) & ";AC,300000;DC,700000;BGB")
        End If

        motionTimer.Start()
    End Sub
    Private Sub cClockwiseABtn_MouseUp(sender As Object, e As MouseEventArgs) Handles cClockwiseABtn.MouseUp
        StopBtn()
    End Sub
    Private Sub plusZBtn_MouseDown(sender As Object, e As MouseEventArgs) Handles plusZBtn.MouseDown
        If RadioButtonJS.Checked Then
            g.command("JG" & Int(jogSpeed * countPerZ * 100) & ",," & Int(jogSpeed * countPerZ * 100) & ";AC300000,,300000;DC700000,,700000;BGAC")
        Else
            g.command("JG" & VS(0, 1) & ",," & VS(0, 1) & ";AC300000,,300000;DC700000,,700000;BGAC")
        End If
        motionTimer.Start()
    End Sub
    Private Sub plusZBtn_MouseUp(sender As Object, e As MouseEventArgs) Handles plusZBtn.MouseUp
        StopBtn()
    End Sub
    Private Sub minusZBtn_MouseDown(sender As Object, e As MouseEventArgs) Handles minusZBtn.MouseDown

        If RadioButtonJS.Checked Then
            g.command("JG-" & Int(jogSpeed * countPerZ * 100) & ",,-" & Int(jogSpeed * countPerZ * 100) & ";AC200000,,200000;DC700000,,700000;BGAC")
        Else
            g.command("JG-" & VS(0, 1) & ",,-" & VS(0, 1) & ";AC300000,,300000;DC700000,,700000;BGAC")
        End If

        motionTimer.Start()
    End Sub
    Private Sub minusZBtn_MouseUp(sender As Object, e As MouseEventArgs) Handles minusZBtn.MouseUp
        StopBtn()
    End Sub
    Private Sub Form1_KeyDown(sender As Object, e As KeyEventArgs) Handles MyBase.KeyDown  'Control motion with Arrows on keyboard
        'make sure KeyPreview is True on Form Properties
        'On Error Resume Next
        If arrowsPanel.Enabled Then
            Select Case e.KeyCode
                Case Keys.Up, Keys.NumPad8 'KeyUp
                    'MsgBox("KeyUp")
                    e.Handled = True
                    plusRBtn_MouseDown(Nothing, Nothing)
                Case Keys.Down, Keys.NumPad5 'KeyDown
                    'MsgBox("KeyDown")
                    e.Handled = True
                    minusRBtn_MouseDown(Nothing, Nothing)
                Case Keys.Left, Keys.NumPad4 'KeyLeft
                    'MsgBox("KeyLeft")
                    e.Handled = True
                    plusZBtn_MouseDown(Nothing, Nothing)
                Case Keys.Right, Keys.NumPad6 'KeyRight
                    'MsgBox("KeyRight")
                    e.Handled = True
                    minusZBtn_MouseDown(Nothing, Nothing)
                Case Keys.End, Keys.NumPad9 'rotate ccw
                    'MsgBox("KeyCCW")
                    e.Handled = True
                    cClockwiseABtn_MouseDown(Nothing, Nothing)

                Case Keys.ControlKey, Keys.NumPad7 'rotate cw
                    'MsgBox("KeyCW")
                    e.Handled = True
                    clockwiseABtn_MouseDown(Nothing, Nothing)
            End Select
        End If

    End Sub
    Private Sub Form1_KeyUp(sender As Object, e As KeyEventArgs) Handles MyBase.KeyUp
        If arrowsPanel.Enabled Then
            Select Case e.KeyCode
                Case Keys.Up, Keys.NumPad8 'KeyUp
                    'MsgBox("KeyUp")
                    e.Handled = True
                    plusRBtn_MouseUp(Nothing, Nothing)
                Case Keys.Down, Keys.NumPad5 'KeyDown
                    'MsgBox("KeyDown")
                    e.Handled = True
                    minusRBtn_MouseUp(Nothing, Nothing)
                Case Keys.Left, Keys.NumPad4 'KeyLeft
                    'MsgBox("KeyLeft")
                    e.Handled = True
                    plusZBtn_MouseUp(Nothing, Nothing)
                Case Keys.Right, Keys.NumPad6 'KeyRight
                    'MsgBox("KeyRight")
                    e.Handled = True
                    minusZBtn_MouseUp(Nothing, Nothing)
                Case Keys.End, Keys.NumPad9 'rotate ccw
                    'MsgBox("KeyCCW")
                    e.Handled = True
                    cClockwiseABtn_MouseUp(Nothing, Nothing)

                Case Keys.ControlKey, Keys.NumPad7 'rotate cw
                    'MsgBox("KeyCW")
                    e.Handled = True
                    clockwiseABtn_MouseUp(Nothing, Nothing)
            End Select
        End If
    End Sub

    'For manually downloading code to controler
    Private Sub codeTextBox_KeyPress(sender As Object, e As KeyPressEventArgs) Handles codeTextBox.KeyPress
        If e.KeyChar = Chr(Keys.Enter) Then
            g.programDownload(codeTextBox.Text) 'Download the contents of the textbox
            g.command("XQ") 'Send execute com   mand
            e.Handled = True
        End If
    End Sub
    '************************************************End of Manual motion, position and GMC programming**********************************************************************************


    'Sub needs update 
    Private Sub RadioButtonGC_CheckedChanged(sender As Object, e As EventArgs) Handles RadioButtonGC.CheckedChanged
        r = Math.Sqrt(X * X + Y * Y)
        angle = Math.Atan2(Y, X) * (180 / Math.PI)
        PAA = angle * countPerDegree
        PAR = r * countPerR
        PAZ = Z * countPerZ
        comTxt = ("PA" & PAR & "," & PAA & "," & PAZ & ";SP" & rSpd & "," & aSpd & "," & zSpd & ";BGABC")
    End Sub
    Private Sub RadioButton1_CheckedChanged(sender As Object, e As EventArgs) Handles RadioButton1.CheckedChanged
        rad = TextBox1.Text * (Math.PI / 180)
        coordXTextBox.Text = TextBox2.Text * Math.Cos(rad)
        coordYTextBox.Text = TextBox2.Text * Math.Sin(rad)
        coordZTextBox.Text = TextBox3.Text
    End Sub

    'Show Form 2
    Private Sub Button8_Click(sender As Object, e As EventArgs) Handles Button8.Click
        Form2.Show()
    End Sub

    '**********************************************************Code involving reading and execution of G-code********************************************************************************************************
    'Open file and display in gCodeTextBox
    Private Sub openFileBtn_Click(sender As Object, e As EventArgs) Handles openFileBtn.Click
        gCodeTextBox.Text = ""
        Dim textLine1, arrayGCode(), lineGCode(), text As String
        text = ""
        OpenFileDialog1.InitialDirectory = "C:\"
        OpenFileDialog1.Title = "Open a Text File"
        OpenFileDialog1.Filter = "G Code Files(*.gcode)|*.gcode|Text Files(*.txt)|*.txt|Numerical Control Files(*.NC)|*.NC"
        OpenFileDialog1.ShowDialog()
        fileName = OpenFileDialog1.FileName
        If System.IO.File.Exists(fileName) = True Then
            Dim objReader As New System.IO.StreamReader(fileName)
            Dim topX, lowerX, topY, lowerY, topZ, lowerZ As Single
            Do While objReader.Peek() <> -1
                textLine1 = objReader.ReadLine()
                arrayGCode = textLine1.Split(";")
                For i As Integer = 0 To UBound(arrayGCode)
                    lineGCode = arrayGCode(i).Split(" ")
                    For k As Integer = 0 To UBound(lineGCode)
                        If lineGCode(k) <> "" Then
                            Select Case lineGCode(k).First
                                Case "G"
                                    If lineGCode(k).Length = 2 Then
                                        lineGCode(k) = lineGCode(k).First + "0" + lineGCode(k).Last
                                        arrayGCode(i) = ""
                                        For j As Integer = 0 To UBound(lineGCode)
                                            arrayGCode(i) += lineGCode(j) + " "
                                        Next j
                                    End If
                                Case "X"
                                    If topX < CSng(lineGCode(k).Remove(0, 1)) Then topX = CSng(lineGCode(k).Remove(0, 1))
                                    If lowerX > CSng(lineGCode(k).Remove(0, 1)) Then lowerX = CSng(lineGCode(k).Remove(0, 1))
                                Case "Y"
                                    If topY < CSng(lineGCode(k).Remove(0, 1)) Then topY = CSng(lineGCode(k).Remove(0, 1))
                                    If lowerY > CSng(lineGCode(k).Remove(0, 1)) Then lowerY = CSng(lineGCode(k).Remove(0, 1))
                                Case "Z"
                                    If topZ < CSng(lineGCode(k).Remove(0, 1)) Then topZ = CSng(lineGCode(k).Remove(0, 1))
                                    If lowerZ > CSng(lineGCode(k).Remove(0, 1)) Then lowerZ = CSng(lineGCode(k).Remove(0, 1))
                            End Select
                        End If
                    Next k
                    While arrayGCode(i).Last = " "
                        arrayGCode(i) = arrayGCode(i).Remove(arrayGCode(i).Length - 1, 1)
                    End While
                    text += arrayGCode(i) & vbNewLine
                Next i
            Loop
            RXLabel.Text += " Max: " & topX & " Min: " & lowerX
            RYLabel.Text += " Max: " & topY & " Min: " & lowerY
            RZLabel.Text += " Max: " & topZ & " Min: " & lowerZ
            objReader.Close()
        End If
        gCodeTextBox.Text = text
        'radius = 3
        'radius0 = 0
        'X = 0
        'Y = 0
        'Z = -3
        'X0 = 0
        'Y0 = 0
        'Z0 = 0
        'theta = 0
        'theta0 = 0
    End Sub

    'Save text in gCodeTextBox as new file
    Private Sub saveAsFileBtn_Click(sender As Object, e As EventArgs) Handles saveAsBtn.Click
        Dim saveAsFileName As String

        SaveFileDialog1.InitialDirectory = "C:\Users\Olivier\Documents\Visual Studio 2012\Projects\CNCLathe_Interface2\GCodes"
        SaveFileDialog1.Title = "Save G Code File"
        SaveFileDialog1.Filter = "G Code Files(*.gcode)|*.gcode"
        SaveFileDialog1.OverwritePrompt = True
        SaveFileDialog1.ShowDialog()
        saveAsFileName = SaveFileDialog1.FileName

        If System.IO.File.Exists(saveAsFileName) = True Then
            Dim objWriter As New System.IO.StreamWriter(saveAsFileName)
            objWriter.Write(gCodeTextBox.Text)
            objWriter.Close()
        End If

    End Sub

    'Save text in gCodeTextBox in file
    Private Sub saveBtn_Click(sender As Object, e As EventArgs) Handles saveBtn.Click
        If fileName = "" Then
            saveAsBtn.PerformClick()
        Else
            Dim objWriter As New System.IO.StreamWriter(fileName)
            objWriter.Write(gCodeTextBox.Text)
            objWriter.Close()
        End If
        saveBtn.Enabled = False

    End Sub

    'Reset file variable 
    Private Sub closeFileBtn_Click(sender As Object, e As EventArgs) Handles closeBtn.Click
        fileName = ""
        gCodeTextBox.Text = ""
        lineindex = 0
        radius = 0
        radius0 = 0
        X = 0
        Y = 0
        Z = 0
        X0 = 0
        Y0 = 0
        Z0 = 0
        theta = 0
        theta0 = 0
        errorA = 0
        errorR = 0
        errorZ = 0
        motionTimer.Stop()
        countPerR = 12800 / 1.714
        countPerZ = 12800 / 91.45
    End Sub

    'Enable/disable nextStepBtn
    Private Sub singleStepCheckBox_CheckedChanged(sender As Object, e As EventArgs) Handles singleStepCheckBox.CheckedChanged
        If singleStepCheckBox.Checked Then
            nextStepBtn.Enabled = True
        Else
            nextStepBtn.Enabled = False
        End If
    End Sub

    'Highlights the current step
    Private Sub nextStepBtn_Click(sender As Object, e As EventArgs) Handles nextStepBtn.Click
        nStep = True
        readNextLine(g.commandValue("LM?"))
    End Sub

    'Function defines the feed rate of the linear segment
    Function VS(dx As Single, dy As Single) As String
        Dim vx, vy As Double
        vx = feedRate * dx / Math.Sqrt(dx ^ 2 + dy ^ 2)
        vy = feedRate * dy / Math.Sqrt(dx ^ 2 + dy ^ 2)
        Return Math.Round(Math.Sqrt(vy ^ 2 + ((vx / 1.714 + vy / 91.45) * 91.45) ^ 2))
    End Function

    'Function Reads a single line of G Code
    Function readNextLine(h As Integer)

        Dim bluePen As New Pen(Color.Blue, 1)

        Dim first As Integer = gCodeTextBox.GetFirstCharIndexFromLine(lineindex - 511 + h)
        Dim last As Integer = gCodeTextBox.GetFirstCharIndexFromLine(lineindex - 511 + h + 1)
        If last = -1 Then last = gCodeTextBox.TextLength
        If first = -1 Then
            comTxt = ""
            startBtn.PerformClick()
            msg = "End of file reached"   ' Define message.
            style = MsgBoxStyle.DefaultButton1 Or MsgBoxStyle.Exclamation Or MsgBoxStyle.OkOnly 'Define style
            title = "G Code Error"   ' Define title.
            response = MsgBox(msg, style, title)  ' Display message.
            arrowsPanel.Enabled = True
        ElseIf first = last Then
            lineindex += 1
        Else
            Dim I, J, K, center As Single
            Dim arrayGCode() As String
            Dim CircleR As Boolean = False
            comTxt = ""
            gCodeTextBox.Select(first, last - first)
            gCodeTextBox.SelectionBackColor = Color.Yellow
            textLine = gCodeTextBox.Lines(lineindex)
            arrayGCode = textLine.Split(" ")

            For g As Integer = 0 To UBound(arrayGCode)
                Select Case arrayGCode(g).First
                    Case "G"
                        gCode = arrayGCode(g).Remove(0, 1)
                        If gCode = "20" Then
                            countPerR = (12800 / 0.067480315)
                            countPerZ = (12800 / 3.6003937)
                        End If
                    Case "X"
                        If RB2D.Checked Then
                            X = CSng(arrayGCode(g).Remove(0, 1)) * scaleTextBox.Text
                        Else
                            X = -CSng(arrayGCode(g).Remove(0, 1)) * scaleTextBox.Text
                        End If
                    Case "Y"
                        Y = CSng(arrayGCode(g).Remove(0, 1)) * scaleTextBox.Text
                    Case "Z"
                        Z = CSng(arrayGCode(g).Remove(0, 1)) * scaleTextBox.Text
                    Case "C"
                        theta = CSng(arrayGCode(g).Remove(0, 1)) * scaleTextBox.Text
                    Case "I"
                        I = CSng(arrayGCode(g).Remove(0, 1)) * scaleTextBox.Text
                    Case "J"
                        J = CSng(arrayGCode(g).Remove(0, 1)) * scaleTextBox.Text
                    Case "K"
                        K = CSng(arrayGCode(g).Remove(0, 1)) * scaleTextBox.Text
                    Case "F"
                        'If arrayGCode(g).Remove(0, 1) < 100 Then
                        'feedRate = arrayGCode(g).Remove(0, 1) * 75
                        'Else
                        'feedRate = arrayGCode(g).Remove(0, 1)
                        'End If
                        msg = "Feed rate has been changed by G Code to " + Str(feedRate) + " Would you like to keep change?" ' Define message.
                        style = MsgBoxStyle.DefaultButton2 Or MsgBoxStyle.Exclamation Or MsgBoxStyle.YesNo 'Define style
                        title = "G Code"   ' Define title.
                        'response = MsgBox(msg, style, title)  ' Display message.
                        'If response = MsgBoxResult.No Then   ' User chose Yes.
                        'feedRate = feedRateTextBox.Text
                        'Else
                        'feedRateTextBox.Text = feedRate
                        'End If
                    Case "R"
                        center = CSng(arrayGCode(g).Remove(0, 1)) * scaleTextBox.Text
                        CircleR = True
                    Case Else
                End Select
            Next g

            If RB3D.Checked Then
                Select Case gCode
                    Case "0", "00"
                        radius = Y
                        errorA += (theta - homeA) * countPerDegree - Math.Round((theta - homeA) * countPerDegree)
                        errorZ += (X - homeZ) * countPerZ - Math.Round((X - homeZ) * countPerZ)
                        errorR += (radius - homeR) * countPerR + (X - homeZ) * countPerZ - Math.Round((radius - homeR) * countPerR + (X - homeZ) * countPerZ)
                        codeTextBox.Text = homeZ & " " & X
                        PAZ = Math.Round((X - homeZ) * countPerZ) + Math.Round(errorZ)
                        PAR = -Math.Round((radius - homeR) * countPerR - (X - homeZ) * countPerZ) - Math.Round(errorR) - Math.Round(errorZ)
                        PAA = Math.Round((theta - homeA) * countPerDegree) + Math.Round(errorA)
                        If Math.Round(errorA) >= 1 Or Math.Round(errorA) <= -1 Then errorA -= Math.Round(errorA)
                        If Math.Round(errorR) >= 1 Or Math.Round(errorR) <= -1 Then errorR -= Math.Round(errorR)
                        If Math.Round(errorZ) >= 1 Or Math.Round(errorZ) <= -1 Then errorZ -= Math.Round(errorZ)
                        comTxt = "PR" & PAR & "," & PAA & "," & PAZ & ";SP" & rSpd & "," & aSpd & "," & zSpd & ";BGABC" & vbCr & "AM;MG ""Motion Completed"""
                        'My.Computer.FileSystem.WriteAllText("C:\Users\Olivier\Documents\Visual Studio 2012\Projects\CNCLathe_Interface2\commandtext.txt", Str(PAR / countPerCMR) + "," + Str(PAA / countPerDegree) + "," + Str(PAZ / countPerCMZ) + ";", True)
                    Case "1", "01"
                        radius = Y
                        errorR += (radius - radius0) * countPerR + (X - X0) * countPerZ - Math.Round((radius - radius0) * countPerR + (X - X0) * countPerZ)
                        errorZ += (X - X0) * countPerZ - Math.Round((X - X0) * countPerZ)
                        errorA += (theta - theta0) * countPerDegree - Math.Round((theta - theta0) * countPerDegree)

                        Dim newCode As String = "LI" & -Math.Round((radius - radius0) * countPerR - (X - X0) * countPerZ) - Math.Round(errorR) - Math.Round(errorZ) & "," & Math.Round((theta - theta0) * countPerDegree) + Math.Round(errorA) & "," & Math.Round((X - X0) * countPerZ) + Math.Round(errorZ)
                        If newCode = "LI0,0,0" Then
                            comTxt = "MG""Motion Completed"""
                        ElseIf textLine.First = "X" Then
                            g.command(newCode & "<" & VS(radius - radius0, X - X0))
                        ElseIf last = gCodeTextBox.TextLength OrElse textLine.First = "G" And gCodeTextBox.Lines(lineindex + 1).First = "G" Then
                            comTxt = "LMABC;" & newCode & ";LE;VS" & VS(radius - radius0, X - X0) & ";VA60000;VD60000;BGS;AM;MG ""Motion Completed"""
                            'My.Computer.FileSystem.WriteAllText("C:\Users\Olivier\Documents\Visual Studio 2012\Projects\CNCLathe_Interface2\commandtext.txt", "LMABC;" & newCode & ";LE;VS" & VS(radius - radius0, X - X0) & ";VA60000;VD60000;BGS;AM;MG ""Motion Completed""" & vbNewLine, True)
                        ElseIf textLine.First = "G" Then
                            g.command("LMABC;" & newCode & "<" & VS(radius - radius0, X - X0) & ";BGS")
                            'My.Computer.FileSystem.WriteAllText("C:\Users\Olivier\Documents\Visual Studio 2012\Projects\CNCLathe_Interface2\commandtext.txt", "LMABC;" & newCode & "<" & VS(radius - radius0, X - X0) & ";BGS" & vbNewLine, True)
                            comTxt = ""
                            startBtn.Text = "STOP"
                            startBtn.BackColor = Color.Red
                            motionTimer.Start()
                            motion = True
                        Else
                            g.command(newCode & "<" & VS(radius - radius0, X - X0))
                            'My.Computer.FileSystem.WriteAllText("C:\Users\Olivier\Documents\Visual Studio 2012\Projects\CNCLathe_Interface2\commandtext.txt", newCode & "<" & VS(radius - radius0, X - X0) & vbNewLine, True)
                        End If
                        If Math.Round(errorA) >= 1 Or Math.Round(errorA) <= -1 Then errorA -= Math.Round(errorA)
                        If Math.Round(errorR) >= 1 Or Math.Round(errorR) <= -1 Then errorR -= Math.Round(errorR)
                        If Math.Round(errorZ) >= 1 Or Math.Round(errorZ) <= -1 Then errorZ -= Math.Round(errorZ)

                        myGraphics.DrawLine(bluePen, New Point(radius0 * Math.Cos(theta0 * Math.PI / 180) + 245, -radius0 * Math.Sin(theta0 * Math.PI / 180) + 240), New Point(radius * Math.Cos(theta * Math.PI / 180) + 245, -radius * Math.Sin(theta * Math.PI / 180) + 240))
                        myGraphics.DrawLine(bluePen, New Point(-X0 + 45, -radius0 * Math.Sin(theta0 * Math.PI / 180) + 565), New Point(-X + 45, -radius0 * Math.Sin(theta * Math.PI / 180) + 565))
                        'My.Computer.FileSystem.WriteAllText("C:\Users\Olivier\Documents\Visual Studio 2012\Projects\CNCLathe_Interface2\commandtext.txt", Str(radius) + "," + Str(theta) + "," + Str(X) + ";", True)

                End Select
            Else
                Select Case gCode
                    Case "0", "00"
                        PAZ = Math.Round((Y - homeZ) * countPerZ)
                        PAR = Math.Round((X - homeR) * countPerR + (Y - homeZ) * countPerZ)
                        comTxt = "PR" & PAR & ",," & PAZ & ";SP" & rSpd & ",," & zSpd & ";BGAC" & vbCr & "AM;MG ""Motion Completed"""
                        'My.Computer.FileSystem.WriteAllText("C:\Users\Olivier\Documents\Visual Studio 2012\Projects\CNCLathe_Interface2\commandtext.txt", Str(PAR / countPerCMR) + "," + Str(PAA / countPerDegree) + "," + Str(PAZ / countPerCMZ) + ";", True)
                    Case "1", "01"
                        Dim newCode As String = ("LI" & Math.Round((X - X0) * countPerR + (Y - Y0) * countPerZ) & ",," & Math.Round((Y - Y0) * countPerZ) & ";")
                        'MsgBox("LI" & Math.Round((X - X0) * countPerR + (Y - Y0) * countPerZ) & ",," & Math.Round((Y - Y0) * countPerZ) & ";")
                        If newCode = "LI0,,0;" Then
                            comTxt = "WT1000;MG ""Motion Completed"""
                        Else
                            comTxt = "LMAC;" & newCode & "LE;VS" & VS(X - X0, Y - Y0) & ";VA300000;VD300000;BGS;AM;MG ""Motion Completed"""
                        End If
                        myGraphics.DrawLine(bluePen, New Point(X0 * 2 + 245, -Y0 * 2 + 240), New Point(X * 2 + 245, -Y * 2 + 240))
                        'My.Computer.FileSystem.WriteAllText("C:\Users\Olivier\Documents\Visual Studio 2012\Projects\CNCLathe_Interface2\commandtext.txt", Str(radius) + "," + Str(theta) + "," + Str(X) + ";", True)
                    Case "2", "02" 'CW
                        If CircleR Then
                            If center > 0 And X < X0 Or center < 0 And X > X0 Then
                                I = 0.5 * (X - X0) + Math.Sqrt(center * center - 0.25 * (X - X0) * (X - X0) - 0.25 * (Y - Y0) * (Y - Y0)) * (((Y0 - Y) / (X - X0)) / Math.Sqrt(((Y0 - Y) / (X - X0)) ^ 2 + 1))
                                J = 0.5 * (Y - Y0) + Math.Sqrt(center * center - 0.25 * (X - X0) * (X - X0) - 0.25 * (Y - Y0) * (Y - Y0)) * (1 / Math.Sqrt(((Y0 - Y) / (X - X0)) ^ 2 + 1))
                            Else
                                I = 0.5 * (X - X0) - Math.Sqrt(center * center - 0.25 * (X - X0) * (X - X0) - 0.25 * (Y - Y0) * (Y - Y0)) * (((Y0 - Y) / (X - X0)) / Math.Sqrt(((Y0 - Y) / (X - X0)) ^ 2 + 1))
                                J = 0.5 * (Y - Y0) - Math.Sqrt(center * center - 0.25 * (X - X0) * (X - X0) - 0.25 * (Y - Y0) * (Y - Y0)) * (1 / Math.Sqrt(((Y0 - Y) / (X - X0)) ^ 2 + 1))
                            End If
                        End If
                        comTxt = "LMAC;" & vbCr
                        Dim aX = X0, aY = Y0, bX, bY As Double
                        Dim angleA, s As Double
                        angleA = Math.Atan2(-J, -I) - Math.Atan2(Y - (Y0 + J), X - (X0 + I))
                        If angleA < 0 Then angleA += 2 * Math.PI
                        While s < angleA
                            s += Math.PI / 350
                            bX = (-I) * Math.Cos(-s) - (-J) * Math.Sin(-s) + X0 + I
                            bY = (-I) * Math.Sin(-s) + (-J) * Math.Cos(-s) + Y0 + J
                            Dim newCode As String = "LI" & Math.Round((bX - aX) * countPerR + (bY - aY) * countPerZ) & ",," & Math.Round((bY - aY) * countPerZ)
                            myGraphics.DrawLine(bluePen, New Point(aX * 2 + 245, -aY * 2 + 240), New Point(bX * 2 + 245, -bY * 2 + 240))
                            If newCode <> "LI0,,0" Then comTxt += newCode & "<" & VS(bX - aX, bY - aY) & vbCr
                            aX = bX
                            aY = bY
                        End While
                        If Math.Round(X - aX) <> 0 Or Math.Round(Y - aY) <> 0 Then
                            comTxt += "LI" & Math.Round((X - aX) * countPerR + (Y - aY) * countPerZ) & ",," & Math.Round((Y - aY) * countPerZ) & "<" & VS(X - aX, bY - aY) & vbCr
                            myGraphics.DrawLine(bluePen, New Point(aX * 2 + 245, -aY * 2 + 240), New Point(X * 2 + 245, -Y * 2 + 240))
                        End If
                        If comTxt = "LMAC;" & vbCr Then
                            comTxt = "WT1000;MG ""Motion Completed"""
                        Else
                            comTxt += ("LE;VS" & feedRate & ";VA300000;VD300000;BGS;AM;MG ""Motion Completed""")
                        End If

                        'My.Computer.FileSystem.WriteAllText("C:\Users\Olivier\Documents\Visual Studio 2012\Projects\CNCLathe_Interface2\commandtext.txt", Str(radius) + "," + Str(theta) + "," + Str(X0) + ";", True)
                    Case "3", "03" 'CCW
                        If CircleR Then
                            If center > 0 And X > X0 Or center < 0 And X > X0 Then
                                I = 0.5 * (X - X0) + Math.Sqrt(center * center - 0.25 * (X - X0) * (X - X0) - 0.25 * (Y - Y0) * (Y - Y0)) * (((Y0 - Y) / (X - X0)) / Math.Sqrt(((Y0 - Y) / (X - X0)) ^ 2 + 1))
                                J = 0.5 * (Y - Y0) + Math.Sqrt(center * center - 0.25 * (X - X0) * (X - X0) - 0.25 * (Y - Y0) * (Y - Y0)) * (1 / Math.Sqrt(((Y0 - Y) / (X - X0)) ^ 2 + 1))
                            Else
                                I = 0.5 * (X - X0) - Math.Sqrt(center * center - 0.25 * (X - X0) * (X - X0) - 0.25 * (Y - Y0) * (Y - Y0)) * (((Y0 - Y) / (X - X0)) / Math.Sqrt(((Y0 - Y) / (X - X0)) ^ 2 + 1))
                                J = 0.5 * (Y - Y0) - Math.Sqrt(center * center - 0.25 * (X - X0) * (X - X0) - 0.25 * (Y - Y0) * (Y - Y0)) * (1 / Math.Sqrt(((Y0 - Y) / (X - X0)) ^ 2 + 1))
                            End If
                        End If
                        comTxt = "LMAC;"
                        Dim aX = X0, aY = Y0, bX, bY As Double
                        Dim angleA, s As Double
                        angleA = Math.Atan2(Y - (Y0 + J), X - (X0 + I)) - Math.Atan2(-J, -I)
                        If angleA < 0 Then angleA += 2 * Math.PI
                        While s < angleA
                            s += Math.PI / 350
                            bX = (-I) * Math.Cos(s) - (-J) * Math.Sin(s) + X0 + I
                            bY = (-I) * Math.Sin(s) + (-J) * Math.Cos(s) + Y0 + J
                            Dim newCode As String = "LI" & Math.Round((bX - aX) * countPerR + (bY - aY) * countPerZ) & ",," & Math.Round((bY - aY) * countPerZ)
                            myGraphics.DrawLine(bluePen, New Point(aX * 2 + 245, -aY * 2 + 240), New Point(bX * 2 + 245, -bY * 2 + 240))
                            If newCode <> "LI0,,0" Then comTxt += newCode & "<" & VS(bX - aX, bY - aY) & vbCr
                            aX = bX
                            aY = bY
                        End While
                        If Math.Round(X - aX) <> 0 Or Math.Round(Y - aY) <> 0 Then
                            comTxt += "LI" & Math.Round((X - aX) * countPerR + (Y - aY) * countPerZ) & ",," & Math.Round((Y - aY) * countPerZ) & "<" & VS(X - aX, bY - aY) & vbCr
                            myGraphics.DrawLine(bluePen, New Point(aX * 2 + 245, aY * 2 + 240), New Point(X * 2 + 245, Y * 2 + 240))
                        End If
                        If comTxt = "LMAC;" & vbCr Then
                            comTxt = "WT1000;MG ""Motion Completed"""
                        Else
                            comTxt += ("LE;VS" & feedRate & ";VA300000;VD300000;BGS;AM;MG ""Motion Completed""")
                        End If
                        'My.Computer.FileSystem.WriteAllText("C:\Users\Olivier\Documents\Visual Studio 2012\Projects\CNCLathe_Interface2\commandtext.txt", Str(radius) + "," + Str(theta) + "," + Str(aX) + ";", True)
                End Select
            End If

            If comTxt <> "" Then
                startBtn.PerformClick()
            End If
            X0 = X
            Y0 = Y
            Z0 = Z
            If theta = 358 Then
                theta0 = -2
            Else
                theta0 = theta
            End If

            radius0 = radius
            lineindex += 1
        End If


    End Function

    'Reads G-Code
    Private Sub readBtn_Click(sender As Object, e As EventArgs) Handles readBtn.Click
        read = True
        gCodeTextBox.Focus()
        readNextLine(g.command("LM?"))
    End Sub

    'Enable/Disable G-Code Buttons
    Private Sub gCodeTextBox_TextChanged(sender As Object, e As EventArgs) Handles gCodeTextBox.TextChanged
        readBtn.Enabled = True
        singleStepCheckBox.Enabled = True
        closeBtn.Enabled = True
        saveAsBtn.Enabled = True
        saveBtn.Enabled = True
        If gCodeTextBox.Text = "" Then
            readBtn.Enabled = False
            singleStepCheckBox.Enabled = False
            closeBtn.Enabled = False
            saveAsBtn.Enabled = False
            saveBtn.Enabled = False
            fileName = ""
        End If
    End Sub
    '**********************************************************End of Code involving reading and execution of G-code********************************************************************************************************


    'HOMING
    Private Sub setHomeButton_Click(sender As Object, e As EventArgs) Handles setHomeButton.Click
        setRHome = -TextBox2.Text
    End Sub
    Private Sub HomingBtn_Click(sender As Object, e As EventArgs) Handles HomingBtn.Click
        homingTimer.Interval = 100
        motionTimer.Stop()
        homingTimer.Start()
        g.programDownload("#HOMEC;homec = 0;homea = 0;CN,1;SP3000,,3000;" & vbCr & "JG3000;HMC;BGAC;AMC;STA;homec=1;EN;" & vbCr & "#HOMEA;homec=0;SP5000;CN,-1;HMA;BGA;AM;WT1000;DP,,0;WT500;homea = 1;EN;")
        g.command("XQ#HOMEC")
    End Sub
    Private Sub homingTimer_Tick(sender As Object, e As EventArgs) Handles homingTimer.Tick
        TextBox3.Text = Math.Round(g.commandValue("TDC") / countPerZ, 3)
        TextBox2.Text = -Math.Round((g.commandValue("TDA") - g.commandValue("TDC")) / countPerR, 3)

        If g.commandValue("MG homec") Then g.command("XQ#HOMEA")
        If g.commandValue("MG homea") Then
            homingTimer.Stop()
            g.command("DP" & Math.Round((-TextBox2.Text - setRHome) * countPerR))
            gCodeTextBox.Text = g.commandValue("TDA") / countPerR
            homeR = -Math.Round((g.commandValue("TDA") - g.commandValue("TDC")) / countPerR, 3)
            homeA = Math.Round(g.commandValue("TDB") / countPerDegree, 3)
            If RB3D.Checked Then
                homeZ = -Math.Round(g.commandValue("TDC") / countPerZ, 3)
            Else
                homeZ = Math.Round(g.commandValue("TDC") / countPerZ, 3)
            End If
            TextBox1.Text = homeA
            TextBox2.Text = homeR
            TextBox3.Text = homeZ
        End If

    End Sub

    Private Sub plusZBtn_Click(sender As Object, e As EventArgs) Handles plusZBtn.Click

    End Sub

    Private Sub plusRBtn_Click(sender As Object, e As EventArgs) Handles plusRBtn.Click

    End Sub

    Private Sub minusZBtn_Click(sender As Object, e As EventArgs) Handles minusZBtn.Click

    End Sub

    Private Sub minusRBtn_Click(sender As Object, e As EventArgs) Handles minusRBtn.Click

    End Sub

    Private Sub coordXTextBox_TextChanged(sender As Object, e As EventArgs) Handles coordXTextBox.TextChanged

    End Sub

    Private Sub TextBox1_TextChanged(sender As Object, e As EventArgs) Handles TextBox1.TextChanged

    End Sub

End Class
